#include "EnvironmentModifierController.h"



void EnvironmentModifierController::putAllModulesToReserve()
{
    // connect real modules to objects and store them into map and stack
    for (id_t i = 1; i <= NUMBER_OF_MODULES; i++)
    {
        Node * root = getFromDef(MODULE_DEF_BASE_NAME + TO_STR(i));
        if (root)
        {
            Field * translation = root->getField("translation");
            Field * rotation = root->getField("rotation");
            
            Position position = Position(
                                 (ARENA_SIZE/2) + SPARE_DISTANCE,
                                 GROUND_HEIGHT,
                                 i);
            
            translation->setSFVec3f(position.getTransaltion());
            rotation->setSFRotation(position.getRotation());
        }
        else
        {
            std::cout << MODULE_DEF_BASE_NAME + TO_STR(i) + " does not exist." << std::endl;
        }
    }
}


void EnvironmentModifierController::putModuleToReserve(std::string moduleName)
{
    for (int i = 1; i <= NUMBER_OF_MODULES; i++)
    {
        Node * root = getFromDef(MODULE_DEF_BASE_NAME + TO_STR(i));
        if (root)
        {
            Field * name = root->getField("name");
            
            if (moduleName.compare(name->getSFString()) == 0)
            {
                Field * controller = root->getField("controller");
                controller->setSFString("void");
                
                Field * controllerArgs = root->getField("controllerArgs");
                controllerArgs->setSFString("");
                
                Field * CB1XLocked = root->getField("CB1XLocked");
                Field * CB1YLocked = root->getField("CB1YLocked");
                Field * CB1ZLocked = root->getField("CB1ZLocked");
                Field * CW1XLocked = root->getField("CW1XLocked");
                Field * CW1ZLocked = root->getField("CW1ZLocked");
                Field * CB2XLocked = root->getField("CB2XLocked");
                Field * CB2ZLocked = root->getField("CB2ZLocked");
                Field * CW2XLocked = root->getField("CW2XLocked");
                Field * CW2YLocked = root->getField("CW2YLocked");
                Field * CW2ZLocked = root->getField("CW2ZLocked");
                
                CB1XLocked->setSFBool(false);
                CB1YLocked->setSFBool(false);
                CB1ZLocked->setSFBool(false);
                CW1XLocked->setSFBool(false);
                CW1ZLocked->setSFBool(false);
                CB2XLocked->setSFBool(false);
                CB2ZLocked->setSFBool(false);
                CW2XLocked->setSFBool(false);
                CW2YLocked->setSFBool(false);
                CW2ZLocked->setSFBool(false);
                
                Field * translation = root->getField("translation");
                Field * rotation = root->getField("rotation");
                
                Position position = Position(
                                             (ARENA_SIZE/2) + SPARE_DISTANCE,
                                             GROUND_HEIGHT,
                                             i);
                
                /*double pos = i;
                double transtlationVector[3] = {(ARENA_SIZE/2) + SPARE_DISTANCE, GROUND_HEIGHT, pos};
                double rotationVector[4] = {1, 1, 1, 0};*/
                
                translation->setSFVec3f(position.getTransaltion());
                rotation->setSFRotation(position.getRotation());
                
                std::cout << moduleName << " to reserve" << std::endl;
                
                sendUpdateAvailableMessageToBirthClinic(MODULE_DEF_BASE_NAME + TO_STR(i));
                
                return;
            }
        }
        else
        {
            std::cout << MODULE_DEF_BASE_NAME + TO_STR(i) + " does not exist." << std::endl;
        }
    }
}


void EnvironmentModifierController::sendInitializedEnvironmentMessage()
{
    emitter->setChannel(EVOLVER_CHANNEL);
    std::string message = "ENVIRONMENT_OK";
    emitter->send(message.c_str(), (int)message.length()+1);
}


void EnvironmentModifierController::sendUpdateAvailableMessageToBirthClinic(std::string moduleDef)
{
    emitter->setChannel(CLINIC_CHANNEL);
    std::string message = "UPDATE_AVAILABLE" + moduleDef;
    emitter->send(message.c_str(), (int)message.length()+1);
}


std::string EnvironmentModifierController::readToReserveMessage(std::string message)
{
    std::string name = message.substr(message.find("TO_RESERVE")+10, message.length());
    return name;
}


//////////////////////////////////////////////////////////////////
///////////////////////// MAIN FUNCTIONS /////////////////////////
//////////////////////////////////////////////////////////////////


EnvironmentModifierController::EnvironmentModifierController()
{
    emitter = getEmitter(EMITTER_NAME);
    emitter->setChannel(EVOLVER_CHANNEL);
    receiver = getReceiver(RECEIVER_NAME);
    receiver->setChannel(MODIFIER_CHANNEL);
}


EnvironmentModifierController::~EnvironmentModifierController()
{
    
}


void EnvironmentModifierController::run()
{
    double timeStep = getBasicTimeStep();
    
    receiver->enable(timeStep);
    while (receiver->getQueueLength() > 0)
    {
        receiver->nextPacket();
    }
    
    putAllModulesToReserve();
    
    sendInitializedEnvironmentMessage();
    
    while (step(timeStep) != -1)
    {
        if(receiver->getQueueLength() > 0)
        {
            std::string message = (char*)receiver->getData();
            
            std::string moduleName = readToReserveMessage(message);
            
            putModuleToReserve(moduleName);
            
            receiver->nextPacket();
        }
    }
}